#Region ;**** Directives created by AutoIt3Wrapper_GUI ****
#AutoIt3Wrapper_Res_requestedExecutionLevel=asInvoker
#EndRegion ;**** Directives created by AutoIt3Wrapper_GUI ****
#include <GUIConstants.au3>
#include 'CommMG.au3';or if you save the commMg.dll in the @scripdir use #include @SciptDir & '\commmg.dll'
#include <GuiEdit.au3>
#include <GuiComboBox.au3>
#include <windowsconstants.au3>
#include <buttonconstants.au3>
#include <misc.au3>
#include <Date.au3>

Global Const $M_PI =  3.14159265358979323846
Global Const $M_PI_2 =  1.57079632679489661923
Global Const $M_PI_4 =  0.785398163397448309616

$logFile = FileOpen("logs/log" & StringReplace(StringReplace(StringReplace(_NowDate() & "." & _NowTime(5), ":", "."), "/", "."), " ", ".") & ".txt", 10)
$tipMode = False

$arduinoBaudRate = 9600
$GPSBaudRate = 4800
$arduinoChannel = 1
$GPSChannel = 2
$motorControl1Channel=3
$motorControl2Channel=4
$motorControl3Channel=5
$motorControl4Channel=6
$lastArduinoComPort = IniRead("defaults.ini", "COM", "arduino", "")
$lastGPSComPort = IniRead("defaults.ini", "COM", "GPS", "")
$lastMC1ComPort = IniRead("defaults.ini", "COM", "Motor Controller 1", "")
$lastMC2ComPort = IniRead("defaults.ini", "COM", "Motor Controller 2", "")
$lastMC3ComPort = IniRead("defaults.ini", "COM", "Motor Controller 3", "")
$lastMC4ComPort = IniRead("defaults.ini", "COM", "Motor Controller 4", "")

$comUpdateTime = 1500 ; milliseconds
$arduinoErrorCount = 0
$GPSErrorCount = 0
$arduinoMaxErrors = 5
$GPSMaxErrors = 5
$scalingFactor = ""
$joyAngle = ""
$joyPosition = ""

$shouldConnect = True
$isConnected = False

$GUI = GUICreate("Umich Rover 2012 - Server", 605, 400)
$IPLabel = GUICtrlCreateLabel("IP Address: " & @IPAddress1, 10, 10, 250, 20)
	GUICtrlSetFont($IPLabel, -1, 800, 4)
$connectedLabel = GUICtrlCreateLabel("Not connceted - Searching for client", 390, 10, 200, 20, 0x0002)
	GUICtrlSetColor($connectedLabel, 0xAA0000)

$arduinoGroup = GUICtrlCreateGroup("Arduino: Channel " & $arduinoChannel, 5, 40, 295, 350)
$arduinoComLabel = GUICtrlCreateLabel("COM port:", 15, 60)
$arduinoCombo = GUICtrlCreateCombo("", 65, 58, 140)
$arduinoStatusLabel = GUICtrlCreateLabel("Not connected", 215, 60, 75, 20, 0x0002)
	GUICtrlSetColor($arduinoStatusLabel, 0xAA0000)
$arduinoCommunication = GUICtrlCreateEdit("", 15, 90, 280, 270)
$arduinoRawInput = GUICtrlCreateInput("", 15, 365, 230, 20)
$arduinoSendButton = GUICtrlCreateButton("Send", 250, 365, 45, 20)


$GPSGroup = GUICtrlCreateGroup("GPS: Channel " & $GPSChannel, 305, 40, 295, 150)
$GPSComLabel = GUICtrlCreateLabel("COM port:", 315, 60)
$GPSCombo = GUICtrlCreateCombo("", 365, 58, 140)
$GPSStatusLabel = GUICtrlCreateLabel("Not connected", 515, 60, 75, 20, 0x0002)
	GUICtrlSetColor($GPSStatusLabel, 0xAA0000)
$GPSCommunication = GUICtrlCreateLabel("", 315, 90, 280, 27)
	GUICtrlSetTip($GPSCommunication, "Click to copy")
$latLabel = GUICtrlCreateLabel("Lat: ", 315, 125, 130, 20, 0x0C)
	GUICtrlSetTip($latLabel, "Click to copy")
$lonLabel = GUICtrlCreateLabel("Lon: ", 315, 145, 130, 20, 0x0C)
	GUICtrlSetTip($lonLabel, "Click to copy")
$speedLabel = GUICtrlCreateLabel("Speed (m/s): ", 455, 125, 100, 20)
	GUICtrlSetTip($speedLabel, "Click to copy")
$altLabel = GUICtrlCreateLabel("Altitude (m): ", 455, 145, 100, 20)
	GUICtrlSetTip($altLabel, "Click to copy")

$mc1Group = GUICtrlCreateGroup("Motor Control 1: Channel " & $motorControl1Channel, 305, 190, 295,50)
$mc1ComLabel = GUICtrlCreateLabel("COM port:", 315, 210)
$mc1Combo = GUICtrlCreateCombo("", 365, 210, 140)
$mc1StatusLabel = GUICtrlCreateLabel("Not connected", 515,210, 75, 20, 0x0002)
	GUICtrlSetColor($mc1StatusLabel, 0xAA0000)

$mc2Group = GUICtrlCreateGroup("Motor Control 2: Channel " & $motorControl2Channel, 305, 240, 295,50)
$mc2ComLabel = GUICtrlCreateLabel("COM port:", 315, 260)
$mc2Combo = GUICtrlCreateCombo("", 365, 260, 140)
$mc2StatusLabel = GUICtrlCreateLabel("Not connected", 515,260, 75, 20, 0x0002)
	GUICtrlSetColor($mc2StatusLabel, 0xAA0000)

$mc3Group = GUICtrlCreateGroup("Motor Control 3: Channel " & $motorControl3Channel, 305, 290, 295,50)
$mc3ComLabel = GUICtrlCreateLabel("COM port:", 315, 310)
$mc3Combo = GUICtrlCreateCombo("", 365, 310, 140)
$mc3StatusLabel = GUICtrlCreateLabel("Not connected", 515,310, 75, 20, 0x0002)
	GUICtrlSetColor($mc3StatusLabel, 0xAA0000)

$mc4Group = GUICtrlCreateGroup("Motor Control 4: Channel " & $motorControl4Channel, 305, 340, 295,50)
$mc4ComLabel = GUICtrlCreateLabel("COM port:", 315, 360)
$mc4Combo = GUICtrlCreateCombo("", 365, 360, 140)
$mc4StatusLabel = GUICtrlCreateLabel("Not connected", 515,360, 75, 20, 0x0002)
	GUICtrlSetColor($mc4StatusLabel, 0xAA0000)

$portlist = _CommListPorts(0);find the available COM ports and write them into the ports combo
If @error = 1 Then
	MsgBox(16, 'Could not read COM ports!','Call Ben on the phone or give up. There is no hope.')
	Exit
EndIf

For $pl = 1 To $portlist[0]
	GUICtrlSetData($arduinoCombo, $portlist[$pl], $lastArduinoComPort)
	GUICtrlSetData($GPSCombo, $portlist[$pl], $lastGPSComPort)
	GUICtrlSetData($mc1Combo,$portlist[$pl],$lastMC1ComPort)
	GUICtrlSetData($mc2Combo,$portlist[$pl],$lastMC2ComPort)
	GUICtrlSetData($mc3Combo,$portlist[$pl],$lastMC3ComPort)
	GUICtrlSetData($mc4Combo,$portlist[$pl],$lastMC4ComPort)
Next

If GUICtrlRead($arduinoCombo) <> "" Then
	$arduinoComPort = GUICtrlRead($arduinoCombo)
EndIf
If GUICtrlRead($GPSCombo) <> "" Then
	$GPSComPort = GUICtrlRead($GPSCombo)
EndIf
If GUICtrlRead($mc1Combo) <> "" Then
	$mc1ComPort = GUICtrlRead($mc1Combo)
EndIf
If GUICtrlRead($mc2Combo) <> "" Then
	$mc2ComPort = GUICtrlRead($mc2Combo)
EndIf
If GUICtrlRead($mc3Combo) <> "" Then
	$mc13ComPort = GUICtrlRead($mc3Combo)
EndIf
If GUICtrlRead($mc4Combo) <> "" Then
	$mc14ComPort = GUICtrlRead($mc4Combo)
EndIf
GUISetState(@SW_SHOW)

If $tipMode Then
	$windowPos = WinGetPos($GUI)
	ToolTip("Enter this IP address" & @CRLF & "into the client program" & @CRLF & "(Click to dismiss)", $windowPos[0] + 25, $windowPos[1] + 50, "", 1, 1)
EndIf

setArduinoComPort()
setGPSComPort()

$updateTime = TimerInit() ;The timer used to control the communicate function
$updateComTime= TimerInit() ;The timer used to control the updating of the GPS and Motor controller status

$szIPADDRESS = @IPAddress1
$nPORT = 33891
TCPStartup()
$MainSocket = TCPListen($szIPADDRESS, $nPORT)
$ConnectedSocket = -1

While True
	$msg = GUIGetMsg()
	communicate()
	If $shouldConnect Then
		$ConnectedSocket = TCPAccept($MainSocket)
		If $ConnectedSocket <> -1 Then
			$shouldConnect = False
			$isConnected = True
			TCPSend($ConnectedSocket, "Connected\")
			; Get IP of client connecting
			$szIP_Accepted = SocketToIP($ConnectedSocket)
			GUICtrlSetData($connectedLabel, "Connected to " & $szIP_Accepted)
			GUICtrlSetColor($connectedLabel, 0x00AA00)
		EndIf
	ElseIf $isConnected Then
	EndIf
	If $tipMode and _IsPressed(01) Then
		ToolTip("")
		$tipMode = False
	EndIf
	If $msg == $arduinoCombo Then
		setArduinoComPort()
		IniWrite("defaults.ini", "COM", "arduino", GUICtrlRead($arduinoCombo))
	ElseIf $msg == $GPSCombo Then
		setGPSComPort()
		IniWrite("defaults.ini", "COM", "GPS", GUICtrlRead($GPSCombo))
	ElseIf $msg == $GPSCommunication Then
		ClipPut(StringReplace(GUICtrlRead($GPSCommunication), ", ", ","))
		MsgBox(0, "Success", ClipGet() & " has been copied to your clipboard")
	ElseIf $msg == $lonLabel Then
		ClipPut(StringReplace(GUICtrlRead($lonLabel), "Lon", "Longitude"))
		MsgBox(0, "Success", ClipGet() & " has been copied to your clipboard")
	ElseIf $msg == $latLabel Then
		ClipPut(StringReplace(GUICtrlRead($latLabel), "Lat", "Latitude"))
		MsgBox(0, "Success", ClipGet() & " has been copied to your clipboard")
	ElseIf $msg == $altLabel Then
		ClipPut(GUICtrlRead($altLabel))
		MsgBox(0, "Success", ClipGet() & " has been copied to your clipboard")
	ElseIf $msg == $speedLabel Then
		ClipPut(GUICtrlRead($speedLabel))
		MsgBox(0, "Success", ClipGet() & " has been copied to your clipboard")
	EndIf
	If StringLen(GUICtrlRead($arduinoCommunication)) > 4096 Then GUICtrlSetData($arduinoCommunication, "")
	If $msg == $GUI_EVENT_CLOSE Then quit()
WEnd

Func communicate()
	If $isConnected Then
		; Receive TCP data
		;If anything is recieved from the Basestation,this loops until all information is recieved
		$recv = TCPRecv($ConnectedSocket, 2048)
TCPSend($ConnectedSocket,"A\12.12341|12.33|1355666|/")
Sleep(1999)
		; If the receive failed with @error then the socket has disconnected
		;----------------------------------------------------------------
		If @error Then
			$ConnectedSocket = -1
			GUICtrlSetData($connectedLabel, "Not connected - Searching for client")
			GUICtrlSetColor($connectedLabel, 0xAA0000)
			Do
				$ConnectedSocket = TCPAccept($MainSocket)
			Until $ConnectedSocket <> -1
			$isConnected = False
			$shouldConnect = True
			Return
		EndIf

		$recievedString= $recv
		While($recv <> "")
			Sleep(10)
			$recv = TCPRecv($ConnectedSocket, 2048)
			$recievedString= $recievedString & $recv
		WEnd
		;If anything is recieved this runs
		If($recievedString<>"")Then
	;			ConsoleWrite("Correct " & $recievedString & @CRLF)
			;This parses $recievedString if it is not empty
			;The recievedString should have the format [Flag]\[Data]/[Flag]\[Data]... etc.
			;Make sure to always send [Flag]\ and end the string with "/"
			$splitRecv = StringSplit($recievedString,"/")
;			ConsoleWrite("Recieved String is not empty" & @CRLF)
			For $count=1 to $splitRecv[0]-1
	;				ConsoleWrite("String Split " &$count & " " &$splitRecv[$count] & @CRLF)
				;This splits the recieved string into its individual components
				;It is split from the "/" character. The returned array is setup so that
				;array[0]= number of elements. Then array[1], array[2], array[3]... Are the individual strings
				;that were recieved
				$flagSplit=StringSplit($splitRecv[$count], "\")
				;If the command "E" was recieved, this sends the exitsafestart() command to the rover
				If($flagSplit[1] =="E") Then
					If arduinoConnected() Then
						_CommSwitch($arduinoChannel)
						_CommSendString("E")
					EndIf
				;If M\[Data]/ was recieved then it parses the data
				ElseIf($flagSplit[1] == "M")Then
					$dataSplit= StringSplit($flagSplit[2],"|")
					;dataSplit[1] is the scaling Factor
					If($dataSplit[0] >= 1)Then
						$scalingFactor = $dataSplit[1]
					EndIf
					;dataSplit[2] is the joystick angle
					If($dataSplit[0] >= 2)Then
						$joyAngle = $dataSplit[2]
					EndIf
					;dataSplit[3] is the joystick's postion
					If($dataSplit[0] >= 3)Then
						$joyPosition = $dataSplit[3]
					EndIf
;					ConsoleWrite($recievedString)
;					ConsoleWrite(@CRLF)

					If arduinoConnected() Then
						_CommSwitch($arduinoChannel)
						Sleep(30)
						TankTurning($arduinoCommunication, $scalingFactor, $joyAngle, $joyPosition)
						$serial = ""
						Sleep(150)
						$serial = _CommGetString()
						If $serial <> "" Then
						GUICtrlSetData($arduinoCommunication,  "Serial > " & $serial & @CRLF, "append")
						EndIf
					EndIf
				EndIf
			Next
		EndIf
	EndIf
	;Reads any data from the Arduino and outputs it to the GUI
	; End TCP receive
	If(TimerDiff($updateComTime) > 1500) Then
		If arduinoConnected() Then
			_CommSwitch($arduinoChannel)
			$serial = ""
			Sleep(30)
			$serial = _CommGetString()
			If $serial <> "" Then
			GUICtrlSetData($arduinoCommunication,  "Serial > " & $serial & @CRLF, "append")
			EndIf
			;Tells the arduino to read the status' of the controllers
			getMCStatus($ConnectedSocket)
		EndIf
		If GPSConnected() Then
			_CommSwitch($GPSChannel)
			$GPSData = _CommGetLine(-1, -1, 250)

			If($GPSData == "") Then
				$GPSErrorCount = $GPSErrorCount + 1
			Else
				$GPSErrorCount = 0
			EndIf
			If @error == 1 Or $GPSErrorCount > $GPSMaxErrors Then
				GUICtrlSetData($GPSStatusLabel, "Not connected")
				GUICtrlSetColor($GPSStatusLabel, 0xAA0000)
			Else
				$GPSData = getGPGGA($GPSData)
				parseGPGGA($ConnectedSocket, $GPSData)
				If StringLen($GPSData) > 20 Then
					FileWriteLine($logFile, _NowTime(5) & "|(GPS)|" & $GPSData)
					GUICtrlSetData($GPSCommunication, StringReplace($GPSData, ",", ", "))
				EndIf
			EndIf
		EndIf
		$updateComTime= Timerinit()
	EndIf
EndFunc

Func parseGPGGA($socketConnected,$data)
	$data = StringSplit($data, ",")
	If $data[0] > 14 Then
		If $data[5] == 0 Then
			ConsoleWrite("Invalid GPS Fix")
			Return
		EndIf
		$latDeg = StringTrimRight($data[3], StringLen($data[3]) - 2)
		$latMin = StringTrimLeft(StringTrimLeft($data[3], 2)/60, 1)
		$lonDeg = StringTrimRight($data[5], StringLen($data[5]) - 3)
		While(StringInStr($lonDeg, "0") == 1)
			$lonDeg = StringTrimLeft($lonDeg, 1)
		WEnd
		$lonMin = StringTrimLeft(StringTrimLeft($data[5], 3)/60, 1)
		$lat = $latDeg + $latMin
		$lon = $lonDeg + $lonMin
		If $data[4] == "S" Then $lat = $lat * -1
		If $data[6] == "W" Then $lon = $lon * -1
		GUICtrlSetData($latLabel, "Lat: " & $lat)
		GUICtrlSetData($lonLabel, "Lon: " & $lon)
		GUICtrlSetData($altLabel, "Altitude (m): " & $data[10])
		TCPSend($socketConnected, "G\" & $lat & "|" & $lon & "|" & $data[10] & "|/")
	EndIf
EndFunc

Func getGPGGA($data)
	$data = StringSplit($data, "$GPGGA", 1)
	If $data[0] > 1 Then
		$data = StringSplit($data[2], "$")
		StringReplace($data[1], ",", ",")
		$commas = @extended
		If $commas <> 14 Then
			Return ""
		Else
			StringReplace($data[1], @CRLF, @CRLF)
			$newline = @extended
;			If $newline == 1 Then Return "$GPGGA,214856.000,4217.5944,N,08342.7085,W,1,06,1.3,273.4,M,-34.0,M,,0000*65" & @CRLF ; For debugging with no GPS reception
			If $newline == 1 Then Return "$GPGGA" & $data[1]
			Return ""
		EndIf
	Else
		Return ""
	EndIf
EndFunc

Func setArduinoComPort()
	$comError = ""
	If GUICtrlRead($arduinoCombo) <> ""  Then
		_CommSwitch($arduinoChannel)
		_CommSetPort(StringReplace(GUICtrlRead($arduinoCombo), "COM", ""), $comError, $arduinoBaudRate)
	Else
		Return
	EndIf
	If $comError <> "" Then
		MsgBox(16, "COM Error" , $comError)
	Else
		GUICtrlSetData($arduinoStatusLabel, "Connected")
		GUICtrlSetColor($arduinoStatusLabel, 0x00AA00)
		GUICtrlSetData($arduinoGroup, "Arduino: Channel " & $arduinoChannel & ", " & GUICtrlRead($arduinoCombo))
		$arduinoErrorCount = 0
	EndIf
EndFunc

Func setGPSComPort()
	$comError = ""
	If GUICtrlRead($GPSCombo) <> "" Then
		_CommSwitch($GPSChannel)
		_CommSetPort(StringReplace(GUICtrlRead($GPSCombo), "COM", ""), $comError, $GPSBaudRate)
	Else
		Return
	EndIf
	If $comError <> "" Then
		MsgBox(16, "COM Error" , $comError)
	Else
		GUICtrlSetData($GPSStatusLabel, "Connected")
		GUICtrlSetColor($GPSStatusLabel, 0x00AA00)
		GUICtrlSetData($GPSGroup, "GPS: Channel " & $GPSChannel & ", " & GUICtrlRead($GPSCombo))
		$GPSErrorCount = 0
	EndIf
EndFunc

Func arduinoConnected()
	Return GUICtrlRead($arduinoStatusLabel) == "Connected"
EndFunc

Func GPSConnected()
	Return GUICtrlRead($GPSStatusLabel) == "Connected"
EndFunc

Func SocketToIP($SHOCKET)
    Local $sockaddr, $aRet

    $sockaddr = DllStructCreate("short;ushort;uint;char[8]")

    $aRet = DllCall("Ws2_32.dll", "int", "getpeername", "int", $SHOCKET, _
            "ptr", DllStructGetPtr($sockaddr), "int*", DllStructGetSize($sockaddr))
    If Not @error And $aRet[0] = 0 Then
        $aRet = DllCall("Ws2_32.dll", "str", "inet_ntoa", "int", DllStructGetData($sockaddr, 3))
        If Not @error Then $aRet = $aRet[0]
    Else
        $aRet = 0
    EndIf

    $sockaddr = 0

    Return $aRet
EndFunc

;This sends messages to the Arduino in order to set the speeds of each motor
;This requires the GUI ($edit), the scalingFactor (max speed limit), the angle of the joystick
;and the position of the joystick.
Func TankTurning($edit, $scalingFactor, $joystickAngle, $position)
	Local $rightSpeedCommand
	Local $leftSpeedCommand
	Local $degreeFreedom  = (2/180)*$M_PI
	Local $degreeFreedomPI_4 = (1/180)*$M_PI
	;Checks if parameters are appropriate, if not kills motors
	if(($scalingFactor <= 0 or $scalingFactor > 32) or($joystickAngle < 0 or $joystickAngle > (2*$M_PI)) or ($position <= 10 or $position > 100)) Then
		_CommSendString("ML+0000R+0000")
	Else
		;Checks if the joystick is at 45, -45, 135 or -135
		If($joystickAngle >= ($M_PI_4-$degreeFreedomPI_4) and $joystickAngle <=($M_PI_4+$degreeFreedomPI_4))Then
			_CommSendString("ML+" & String(StringFormat("%04s",$position*$scalingFactor)) & "R" & "+0000")

		ElseIf($joystickAngle >= (7*$M_PI_4-$degreeFreedomPI_4) and $joystickAngle <=(7*$M_PI_4+$degreeFreedomPI_4))Then
			_CommSendString("ML+0000R-" & String(StringFormat("%04s",$position*$scalingFactor)))

		ElseIf($joystickAngle >= 3*$M_PI_4-$degreeFreedomPI_4 and $joystickAngle <= 3*$M_PI_4+$degreeFreedomPI_4)Then
			_CommSendString("ML+0000R+" & String(StringFormat("%04s",$position*$scalingFactor)))

		ElseIf($joystickAngle >= 5*$M_PI_4-$degreeFreedomPI_4 and $joystickAngle <= 5*$M_PI_4+$degreeFreedomPI_4)Then
			_CommSendString("ML-" & String(StringFormat("%04s",$position*$scalingFactor)) & "R" & "+0000")

		;Checks if the joystick is at 0 or 180 degrees
		ElseIf($joystickAngle > -$degreeFreedom and $joystickAngle < $degreeFreedom) Then
			_CommSendString("ML+" & String(StringFormat("%04s", $position*$scalingFactor)) & "R-" & String(StringFormat("%04s",$position*$scalingFactor)))

		ElseIf($joystickAngle < $M_PI+$degreeFreedom and $joystickAngle > $M_PI-$degreeFreedom) Then
			_CommSendString("ML-" & String(StringFormat("%04s", $position*$scalingFactor)) & "R+" & String(StringFormat("%04s",$position*$scalingFactor)))

		;Checks if the joystick is at 90 or -90 degrees
		ElseIf($joystickAngle < $M_PI_2+$degreeFreedom and $joystickAngle > $M_PI_2-$degreeFreedom)Then
			_CommSendString("ML+" & String(StringFormat("%04s", $position*$scalingFactor)) & "R+" & String(StringFormat("%04s",$position*$scalingFactor)))

		ElseIf($joystickAngle < 3*$M_PI_2+$degreeFreedom and $joystickAngle > 3*$M_PI_2-$degreeFreedom)Then
			_CommSendString("ML-" & String(StringFormat("%04s", $position*$scalingFactor)) & "R-" & String(StringFormat("%04s",$position*$scalingFactor)))

		Else

			$joystickAngle=($joystickAngle-$M_PI_4)
			;If joystick is between Pi/4 and Pi/2
			If($joystickAngle > $degreeFreedomPI_4 and $joystickAngle < $M_PI_4-$degreeFreedom) Then
				$leftSpeedCommand= $position*$scalingFactor
				$rightSpeedCommand= Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML+" & String(StringFormat("%04s", $leftSpeedCommand)) & "R+" & String(StringFormat("%04s", $rightSpeedCommand)))
			;If joystick is between Pi/2 and 3Pi/4
			ElseIf($joystickAngle > $M_PI_4+$degreeFreedom and $joystickAngle < $M_PI_2-$degreeFreedomPI_4) Then
				$rightSpeedCommand= $position*$scalingFactor
				$leftSpeedCommand= Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML+" & String(StringFormat("%04s", $leftSpeedCommand)) & "R+" & String(StringFormat("%04s", $rightSpeedCommand)))
			;If joystick is between 3Pi/4 and Pi
			ElseIf($joystickAngle > $M_PI_2+$degreeFreedomPI_4 and $joystickAngle < 3*$M_PI_4-$degreeFreedom)Then
				$rightSpeedCommand= $position*$scalingFactor
				$leftSpeedCommand= -1*Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML-"& String(StringFormat("%04s", $leftSpeedCommand)) & "R+" & String(StringFormat("%04s", $rightSpeedCommand)))
			;If joystick is between Pi and 5Pi/4
			ElseIf($joystickAngle > 3*$M_PI_4+$degreeFreedom and $joystickAngle < $M_PI-$degreeFreedomPI_4)Then
				$leftSpeedCommand= $position*$scalingFactor
				$rightSpeedCommand= -1*Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML-" & String(StringFormat("%04s", $leftSpeedCommand)) & "R+" & String(StringFormat("%04s", $rightSpeedCommand)))
			;If joystick is between 5Pi/4 and 3Pi/2
			ElseIf($joystickAngle > $M_PI+$degreeFreedomPI_4 and $joystickAngle < 5*$M_PI_4-$degreeFreedom)Then
				$leftSpeedCommand= $position*$scalingFactor
				$rightSpeedCommand= Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML-" & String(StringFormat("%04s", $leftSpeedCommand)) & "R-" & String(StringFormat("%04s", $rightSpeedCommand)))
			;If joystick is between 3Pi/2 and 7Pi/4
			ElseIf($joystickAngle > 5*$M_PI_4+$degreeFreedom and $joystickAngle < 3*$M_PI_2-$degreeFreedomPI_4)Then
				$rightSpeedCommand= $position*$scalingFactor
				$leftSpeedCommand= Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML-" & String(StringFormat("%04s", $leftSpeedCommand)) & "R-" & String(StringFormat("%04s", $rightSpeedCommand)))
			;If joystick is between 7Pi/4 and 2Pi
			ElseIf($joystickAngle > 3*$M_PI_2+$degreeFreedomPI_4 and $joystickAngle < 7*$M_PI_4-$degreeFreedom)Then
				$rightSpeedCommand= $position*$scalingFactor
				$leftSpeedCommand= -1*Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML+" & String(StringFormat("%04s", $leftSpeedCommand)) & "R-" & String(StringFormat("%04s", $rightSpeedCommand)))
			;If joystick is between 0 and Pi/4
			ElseIf($joystickAngle > -$M_PI_4+$degreeFreedom and $joystickAngle < 0-$degreeFreedomPI_4) Then
				$leftSpeedCommand= $position*$scalingFactor
				$rightSpeedCommand= -1*Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML+" & String(StringFormat("%04s", $leftSpeedCommand)) & "R-" & String(StringFormat("%04s", $rightSpeedCommand)))

			EndIf
		EndIf
	EndIf
EndFunc

Func getMCStatus($socketConnected)
	Local $recievedString
	Local $recv
	_CommSendString('S');
	Sleep(150)
	$recv = _CommGetString()
	$recievedString= $recv
	;Loops until all data is sent to the rover from the arduino
	While($recv <> "")
;	ConsoleWrite("Recieved a string")
		Sleep(100)
		$recv = _CommGetString()
		$recievedString = $recievedString & $recv
		ConsoleWrite($recievedString & @CRLF)
	WEnd
	;Parses the recieved data into information about each motor controller
	If $recievedString <> "" Then
		$statusArray= StringSplit($recievedString, "/")
		;Sends information about the motor controllers, one at a time
		;It doesn't send them in one transmission, just in case something happens
		;during the transmission. Sending them one at a time gives a better chance of
		;information being recieved at the Basestation
		For $sendCount = 1 to $statusArray[0] - 1
			TCPSend($socketConnected, "ST" & "\" & $sendCount & $statusArray[$sendCount] & "/")
			ConsoleWrite($statusArray[$sendCount] & @CRLF)
			Sleep(2)
		Next
	EndIf
	$recievedString= ""
	$recv = ""
EndFunc

Func quit()
	FileClose($logFile)
	TCPShutdown()
	Exit
EndFunc